#include <math.h>
#include "FOC.h"
#include "FOC_conf.h"
#include "FOC_utils.h"
#include "hardware_api.h"
#include "stdlib.h"
#include "AS5600.h"
#include "stdio.h"
#include "FOC_LPF.h"
#include "time_utils.h"

uint32_t pre_tick=0;
FOC_CONTROL_MODE mode = OPEN_LOOP_POSITION_CONTROL;
float target_velocity=0;
float target_angle=0;

float zero_electrical_angle=0;
float zero_electrical_offset=NOT_SET;//5.54209   5.58505 5.69245
float last_mechanical_angle=0;

struct_FOC_t st_FOC={
        .Ts=0.001,
        .Pn=7,
        .Kv=190,
        .R=10,
        .voltageUsedForSensorAlign=1.0f,
        .PowerVoltage=12,
        .LimitVoltage=8,
        .LimitVelocity=3,
        .LimitCurrent=0.1,
};

void FOC_init() {
  AS5600_Init();
  _init3PWM();
  FOC_lpf_set_Parameters();
  last_mechanical_angle=get_mechanical_angle();
  pre_tick=HAL_GetTick();
  st_FOC.pid_velocity.kp=0.08;
  st_FOC.pid_velocity.ki=0.03;
  st_FOC.pid_velocity.kd=0;
  st_FOC.pid_velocity.min_integral=500;
  st_FOC.pid_velocity.min_integral=-500;
  st_FOC.pid_velocity.max_u=8;
  st_FOC.pid_velocity.min_u=-8;
  st_FOC.pid_velocity.dead_zone=0;

  st_FOC.pid_position.kp = 10.0; // 20.0
  st_FOC.pid_position.ki = 0.0;
  st_FOC.pid_position.kd = 0;
  st_FOC.pid_position.integral = 0;
  st_FOC.pid_position.last_err = 0;
  st_FOC.pid_position.max_integral = 0.0;
  st_FOC.pid_position.min_integral = -0.0;
  st_FOC.pid_position.max_u = 20.0;
  st_FOC.pid_position.min_u = -20.0;
  st_FOC.pid_position.dead_zone = 0.0;
}
void BLDC_Ctrl(FOC_CONTROL_MODE mode){
  switch (mode) {
    case OPEN_LOOP_SPEED_CONTROL:
      velocityOpenLoop(target_velocity);
      break;
    case OPEN_LOOP_POSITION_CONTROL:
      angelOpenLoop(target_angle);
      break;
    case SPEED_CONTROL:
      velocityCloseLoop(target_velocity);
      break;
    case POSITION_CONTROL:
      positionCloseLoop(target_angle);
      break;
  }
}


//[0-4096] =>[0-2PI]  data/max*2PI
//value [0-2PI] 定义传感器读数0为机械角0
float get_mechanical_angle() {
  return ((float)AS5600_getRawData() /(float)SENSOR_VALUE_RANGE)*_2PI;
}
//
float get_electrical_angle() {
  return _normalizeAngle(SENSOR_DIRECTION*st_FOC.Pn * get_mechanical_angle()-zero_electrical_offset );
}
float get_velocity(void){
  float ts=(float)(HAL_GetTick()-pre_tick)*1e-3;
  if (ts<1e-3) ts=1e-3;
  pre_tick=HAL_GetTick();

  float now_mechanical_angle = get_mechanical_angle();
  float delta_angle=now_mechanical_angle-last_mechanical_angle;
  last_mechanical_angle=now_mechanical_angle;
  if (fabs(delta_angle)>(_2PI*0.8)){
    if (delta_angle>0)
      delta_angle-=_2PI;
    else
      delta_angle+=_2PI;
  }
  return delta_angle/ts;
}
//Encoder alignment to electrical 0 angle
float ang;
void alignSensor(void){
  // Align the electrical phases of the motor and sensor
  // Set angle -90(270 = 3PI/2) degrees
  if (!_isset(zero_electrical_offset)){
    setPhaseVoltage(2, 0, _3PI_2);
    HAL_Delay(1000);
     ang= get_mechanical_angle();
    zero_electrical_offset=0;
    zero_electrical_offset=get_electrical_angle();
    setPhaseVoltage(0,0,0);
    HAL_Delay(200);
  }
}
//rad/s

void velocityCloseLoop(float target_velocity){
  static float now_velocity;
  now_velocity = low_pass_filter(&lpf_velocity, get_velocity());
  float Uq = pid_calc(&st_FOC.pid_velocity, target_velocity, now_velocity);
  float electrical_angle = get_electrical_angle();
  setPhaseVoltage(Uq, 0, electrical_angle);
}
void positionCloseLoop(float target_angle){
  target_angle = _normalizeAngle(target_angle);
  float now_angle = _normalizeAngle(get_mechanical_angle()-1.26);

  float angle_error = target_angle-now_angle;
  if (angle_error < -_PI) target_angle += _2PI;
  else if (angle_error > _PI) target_angle -= _2PI;
  float target_vel = pid_calc(&st_FOC.pid_position,target_angle,now_angle);
  float now_velocity = low_pass_filter(&lpf_velocity,get_velocity());
  float uq = pid_calc(&st_FOC.pid_velocity,target_vel,now_velocity);
  float electrical_angle = get_electrical_angle();
  setPhaseVoltage(uq,0,electrical_angle);

}
float angle=0;
//空间弧度转换为电角度
uint64_t openLoopTimeStamp;
void velocityOpenLoop(float target_velocity){
  angle = angle+target_velocity*st_FOC.Ts;   //这里是vel是rad/s  一圈是2π rad 之前理解错了 这个要和下面的电角度区分开
  angle = _normalizeAngle(angle);    //规格化到【0-2PI】 机械角
  //use voltage limit or current limit
  float Uq = _isset(st_FOC.R)?st_FOC.LimitCurrent*st_FOC.R:st_FOC.LimitVoltage;
  //传入参数是电角度
  //所以传入参数是[0-2PI*Pn]
  //一圈的电角度 angle_el=angle*Pn
  setPhaseVoltage(Uq,0, _electricalAngle(angle,st_FOC.Pn));
}
//传入期望的角度 一圈的机械角度[0-2PI]
//不知道为啥  开环角度会有误差  直接输入电角度没有误差  输入机械角度会有挺大的误差
float open_pos=0;
void angelOpenLoop(float target_angle){
  //期望角度和实际角度差 大于限制角速度*ts
  //根据角度差± 来计算±限制的角度
  if (abs(target_angle-open_pos)> abs(st_FOC.LimitVelocity*st_FOC.Ts)){
    open_pos+= _sign(target_angle-open_pos)* abs(st_FOC.LimitVelocity)*st_FOC.Ts;
  } else{
    open_pos=target_angle;
  }
  // angle%2Pi*Pn
  setPhaseVoltage(0,1, _electricalAngle(_normalizeAngle(open_pos),st_FOC.Pn));
 // setPhaseVoltage(1,0,open_pos);
}
uint32_t gTicks=0;
//float setSin(void){
//  if (g_SysTick-g_FocInterval<(UINT32_MAX>>1)){
//    gTicks++;
//    testAngle=_2PI*3.5* sin(_2PI*gTicks/1000);
//    setPhaseVoltage(1,0,testAngle);
//    g_FocInterval=g_SysTick+5;
//  }
//  return testAngle;
//}
//相电压计算算法  传入电角度angle_el
//在一个电角度周期计算相电压合成矢量
void setPhaseVoltage(float Uq, float Ud, float angle_el) {
  int sector;
  float Uout;
  //如果ud设置了 计算uout
  if (Ud!=0){
    Uout=_sqrt(Ud*Ud+Uq*Uq)/st_FOC.PowerVoltage;   ///LimitVoltage/PowerVoltage  max:PowerVoltage
    angle_el= _normalizeAngle(angle_el+ atan2(Uq,Ud));
  } else{
    Uout=Uq/st_FOC.PowerVoltage;
    //电角度又变成[0-2PI] 但是加了PI_2 就是相对于当前电角度90度的提前量
    //我理解是电角度归一化到0-360度
    angle_el= _normalizeAngle(angle_el+_PI_2);  //angle+pi_2)%2pi
  }
    // find the sector we are in currently
    sector = floor(angle_el / _PI_3) + 1;    //向下取整 angle/(2PI/6)+1
    //printf("s:%d=a:%.3f\n",sector,angle_el);
    // calculate the duty cycles
    float T1 = _SQRT3 * _sin(sector * _PI_3 - angle_el) * Uout;
    float T2 = _SQRT3 * _sin(angle_el - (sector - 1.0f) * _PI_3) * Uout;
    float T0 = 1 - T1 - T2; // modulation_centered around driver->voltage_limit/2
    // calculate the duty cycles(times)
    float Ta, Tb, Tc;
    switch (sector) {
        case 1:
            Ta = T1 + T2 + T0 / 2;
            Tb = T2 + T0 / 2;
            Tc = T0 / 2;
            break;
        case 2:
            Ta = T1 + T0 / 2;
            Tb = T1 + T2 + T0 / 2;
            Tc = T0 / 2;
            break;
        case 3:
            Ta = T0 / 2;
            Tb = T1 + T2 + T0 / 2;
            Tc = T2 + T0 / 2;
            break;
        case 4:
            Ta = T0 / 2;
            Tb = T1 + T0 / 2;
            Tc = T1 + T2 + T0 / 2;
            break;
        case 5:
            Ta = T2 + T0 / 2;
            Tb = T0 / 2;
            Tc = T1 + T2 + T0 / 2;
            break;
        case 6:
            Ta = T1 + T2 + T0 / 2;
            Tb = T0 / 2;
            Tc = T1 + T0 / 2;
            break;
        default:
            // possible error state
            Ta = 0;
            Tb = 0;
            Tc = 0;
    }

    // Ta, Tb, Tc range [0,1]

    _writeDutyCycle3PWM(Ta, Tb, Tc);
}
